#include "usbcamera.hpp"

#include <stdexcept>

#include "tools/logger.hpp"
#include "tools/yaml.hpp"

using namespace std::chrono_literals;

namespace io
{
USBCamera::USBCamera(const std::string & open_name, const std::string & config_path)
: open_name_(open_name), quit_(false), ok_(false), queue_(1), open_count_(0)
{
  auto yaml = tools::load(config_path);
  image_width_ = tools::read<double>(yaml, "image_width");
  image_height_ = tools::read<double>(yaml, "image_height");
  usb_exposure_ = tools::read<double>(yaml, "usb_exposure");
  usb_frame_rate_ = tools::read<double>(yaml, "usb_frame_rate");
  usb_gamma_ = tools::read<double>(yaml, "usb_gamma");
  usb_gain_ = tools::read<double>(yaml, "usb_gain");
  try_open();

  // 守护线程
  daemon_thread_ = std::thread{[this] {
    // tools::logger()->info("daemon thread start");
    while (!quit_) {
      std::this_thread::sleep_for(100ms);

      if (ok_) continue;

      if (open_count_ > 20) {
        tools::logger()->warn("Give up to open {} USB camera", this->device_name);
        quit_ = true;

        {
          std::lock_guard<std::mutex> lock(cap_mutex_);
          close();  // 先关闭摄像头
        }

        if (capture_thread_.joinable()) {
          tools::logger()->warn("Stopping capture thread");
          capture_thread_.join();
        }

        break;
      }

      if (capture_thread_.joinable()) capture_thread_.join();

      {
        std::lock_guard<std::mutex> lock(cap_mutex_);
        close();
      }
      try_open();
    }
    // tools::logger()->info("daemon thread exit");
  }};
}

USBCamera::~USBCamera()
{
  quit_ = true;
  {
    std::lock_guard<std::mutex> lock(cap_mutex_);
    close();
  }
  if (daemon_thread_.joinable()) daemon_thread_.join();
  if (capture_thread_.joinable()) capture_thread_.join();
  tools::logger()->info("USBCamera destructed.");
}

cv::Mat USBCamera::read()
{
  std::lock_guard<std::mutex> lock(cap_mutex_);
  if (!cap_.isOpened()) {
    tools::logger()->warn("Failed to read {} USB camera", this->device_name);
    return cv::Mat();
  }
  cap_ >> img_;
  return img_;
}

void USBCamera::read(cv::Mat & img, std::chrono::steady_clock::time_point & timestamp)
{
  CameraData data;
  queue_.pop(data);

  img = data.img;
  timestamp = data.timestamp;
}

void USBCamera::open()
{
  std::lock_guard<std::mutex> lock(cap_mutex_);
  std::string true_device_name = "/dev/" + open_name_;
  cap_.open(true_device_name, cv::CAP_V4L);
  if (!cap_.isOpened()) {
    // 增强错误诊断信息
    tools::logger()->error("Failed to open USB camera - Device: {}", true_device_name);
    tools::logger()->error("Diagnostic info:");
    tools::logger()->error("  1. Check if device exists: ls -la {}", true_device_name);
    tools::logger()->error("  2. Check device permissions: groups $(whoami)");
    tools::logger()->error("  3. Check if device is busy: lsof | grep {}", open_name_);
    tools::logger()->error("  4. Check UVC driver: lsmod | grep uvcvideo");
    tools::logger()->error("  5. Check device capabilities: v4l2-ctl --device={} --all", true_device_name);
    tools::logger()->error("  6. Try manual test: v4l2-ctl --device={} --stream-mmap --stream-count=1", true_device_name);
    return;
  }
  
  // 获取摄像头基本信息
  sharpness_ = cap_.get(cv::CAP_PROP_SHARPNESS);
  double native_width = cap_.get(cv::CAP_PROP_FRAME_WIDTH);
  double native_height = cap_.get(cv::CAP_PROP_FRAME_HEIGHT);
  double native_fps = cap_.get(cv::CAP_PROP_FPS);
  
  tools::logger()->info("USB Camera detected - Device: {}, Sharpness: {}, Native: {}x{}@{:.1f}fps", 
                       open_name_, sharpness_, native_width, native_height, native_fps);

  // 智能摄像头类型检测
  bool is_high_res_camera = (native_width >= 1920 || native_height >= 1080);
  bool is_industrial_camera = (sharpness_ == -1 || sharpness_ > 100 || is_high_res_camera);
  
  if (is_industrial_camera) {
    // 工业/高清摄像头专用配置
    device_name = "industrial";
    tools::logger()->info("Configuring as industrial/high-res camera");
    
    // 格式选择策略
    std::vector<std::pair<std::string, int>> formats = {
      {"MJPG", cv::VideoWriter::fourcc('M', 'J', 'P', 'G')},
      {"YUYV", cv::VideoWriter::fourcc('Y', 'U', 'Y', 'V')}
    };
    
    bool format_configured = false;
    for (auto& [format_name, format_code] : formats) {
      // 先设置格式
      cap_.set(cv::CAP_PROP_FOURCC, format_code);
      
      // 再设置分辨率（根据格式调整）
      int target_width = image_width_;
      int target_height = image_height_;
      double target_fps = usb_frame_rate_;
      
      // YUYV格式高分辨率帧率限制
      if (format_name == "YUYV" && (target_width > 1280 || target_height > 720)) {
        target_fps = 10.0;  // YUYV高分辨率降低帧率
        tools::logger()->warn("YUYV format with high resolution, reducing fps to {}", target_fps);
      }
      
      // 设置分辨率
      cap_.set(cv::CAP_PROP_FRAME_WIDTH, target_width);
      cap_.set(cv::CAP_PROP_FRAME_HEIGHT, target_height);
      
      // 验证分辨率设置
      double actual_width = cap_.get(cv::CAP_PROP_FRAME_WIDTH);
      double actual_height = cap_.get(cv::CAP_PROP_FRAME_HEIGHT);
      
      if (actual_width == target_width && actual_height == target_height) {
        // 设置帧率
        cap_.set(cv::CAP_PROP_FPS, target_fps);
        double actual_fps = cap_.get(cv::CAP_PROP_FPS);
        
        tools::logger()->info("Industrial camera configured - Format: {}, Resolution: {}x{}@{}fps",
                             format_name, actual_width, actual_height, actual_fps);
        format_configured = true;
        break;
      } else {
        tools::logger()->warn("Format {} failed to set resolution {}x{}, got {}x{}", 
                             format_name, target_width, target_height, actual_width, actual_height);
      }
    }
    
    if (!format_configured) {
      tools::logger()->error("Failed to configure industrial camera with any format");
      // 使用原生参数作为fallback
      cap_.set(cv::CAP_PROP_FRAME_WIDTH, native_width);
      cap_.set(cv::CAP_PROP_FRAME_HEIGHT, native_height);
      cap_.set(cv::CAP_PROP_FPS, native_fps);
      tools::logger()->warn("Using native configuration: {}x{}@{}fps", 
                           native_width, native_height, native_fps);
    }
    
    // 工业摄像头参数优化
    try {
      cap_.set(cv::CAP_PROP_AUTO_EXPOSURE, 0.25);  // 手动曝光
      cap_.set(cv::CAP_PROP_EXPOSURE, usb_exposure_);
      cap_.set(cv::CAP_PROP_GAMMA, usb_gamma_);
      cap_.set(cv::CAP_PROP_GAIN, usb_gain_);
      cap_.set(cv::CAP_PROP_BRIGHTNESS, 50);
      cap_.set(cv::CAP_PROP_CONTRAST, 50);
      cap_.set(cv::CAP_PROP_SATURATION, 60);
    } catch (...) {
      tools::logger()->debug("Some industrial camera parameters not supported");
    }
    
  } else {
    // 原有逻辑（sharpness=2,3等）
    if (sharpness_ == 2) {
      device_name = "left";
    } else if (sharpness_ == 3) {
      device_name = "right";
    } else {
      device_name = "generic";
    }
    
    // 原有配置逻辑
    cap_.set(cv::CAP_PROP_FOURCC, cv::VideoWriter::fourcc('M', 'J', 'P', 'G'));
    cap_.set(cv::CAP_PROP_FPS, usb_frame_rate_);
    cap_.set(cv::CAP_PROP_FRAME_WIDTH, image_width_);
    cap_.set(cv::CAP_PROP_FRAME_HEIGHT, image_height_);
    cap_.set(cv::CAP_PROP_EXPOSURE, usb_exposure_);
    cap_.set(cv::CAP_PROP_GAMMA, usb_gamma_);
    cap_.set(cv::CAP_PROP_GAIN, usb_gain_);
  }

  // 最终状态验证
  double final_width = cap_.get(cv::CAP_PROP_FRAME_WIDTH);
  double final_height = cap_.get(cv::CAP_PROP_FRAME_HEIGHT);
  double final_fps = cap_.get(cv::CAP_PROP_FPS);
  int final_fourcc = cap_.get(cv::CAP_PROP_FOURCC);
  
  char fourcc_str[5] = {0};
  sprintf(fourcc_str, "%c%c%c%c", 
          (final_fourcc >> 0) & 0xFF, (final_fourcc >> 8) & 0xFF,
          (final_fourcc >> 16) & 0xFF, (final_fourcc >> 24) & 0xFF);
  
  tools::logger()->info("{} USBCamera opened successfully", device_name);
  tools::logger()->info("Final configuration - Resolution: {}x{}@{}fps, Format: {}, Sharpness: {}",
                       final_width, final_height, final_fps, fourcc_str, sharpness_);

  // 取图线程
  capture_thread_ = std::thread{[this] {
    ok_ = true;
    std::this_thread::sleep_for(50ms);
    tools::logger()->info("[{} USB camera] capture thread started ", this->device_name);
    while (!quit_) {
      std::this_thread::sleep_for(1ms);

      cv::Mat img;
      bool success;
      {
        std::lock_guard<std::mutex> lock(cap_mutex_);
        if (!cap_.isOpened()) {
          break;
        }
        success = cap_.read(img);
      }

      if (!success) {
        tools::logger()->warn("Failed to read frame, exiting capture thread");
        break;
      }

      auto timestamp = std::chrono::steady_clock::now();
      queue_.push({img, timestamp});
    }
    ok_ = false;
  }};
}

void USBCamera::try_open()
{
  try {
    open();
    open_count_++;
  } catch (const std::exception & e) {
    tools::logger()->warn("{}", e.what());
  }
}

void USBCamera::close()
{
  if (cap_.isOpened()) {
    cap_.release();
    tools::logger()->info("USB camera released.");
  }
}

}  // namespace io